

#include <swarm.h>
#include <swarmio.h>

#include <unistd.h>
#include <sys/timeb.h>

int _sw_compare_times(struct timeb *t1, struct timeb *t2){

  int t1_millis = (t1->time * 1000) + t1->millitm;
  int t2_millis = (t2->time * 1000) + t2->millitm;
  
  return t2_millis - t1_millis;

}

int sw_simulate(){

  if (internal_sw == NULL) return 1;

  /* This is the function that does all the magic. */
  printf("[simulator] Starting simulation...\n");
  
  double current_t = 0;

  int time_step = 0;
  int time_steps = (internal_sw->sim_len / internal_sw->tau);

  double frame_delta = (1.0 / internal_sw->fps) * 1000; // milliseconds

  printf("[simulator] frame_delta: %lf\n", frame_delta);
  printf("[simulator] time_steps: %d\n", time_steps);
  printf("[simulator] sim_len: %lf\n", internal_sw->sim_len);
  printf("[simulator] tau: %lf\n", internal_sw->tau);

  sw_display_swarm(internal_sw);
  sw_flush_display();

  while (time_step <= time_steps || time_steps == 0){

    /* Find the start time; useful for determining fps. */
    struct timeb start;
    ftime(&start);

    /* Run a frame. */
    _sw_simulate_frame();

    /* Now find the end time. */
    struct timeb end;
    ftime(&end);

    int frame_time = _sw_compare_times(&start, &end);
    int wait_time = frame_delta - frame_time;

    //printf("[simulator]   Waiting for %d usecs.\n", (wait_time * 1000) + 1);
    usleep((wait_time * 1000) + 1);

    /* Now display the frame. */
    sw_clear_display();
    sw_display_swarm(internal_sw);
    sw_flush_display();

    time_step++;
    current_t += internal_sw->tau;
    /*
    printf("[simulator]   Frame num: %d\n", frame_num++);
    printf("[simulator]   Frame done, current time: %lfs\n", current_t);
    printf("[simulator]   sim_len=%lf  internal->sim_len=%lf\n",
	   sim_len, internal_sw->sim_len);
    */
  }


  return 0;

}

void _sw_load_sym_table_data(sw_swarm *sw){

  double tau = 1;
  double entropy = 1;
  double sim_len = 0;
  int    threads = _SWARM_PROCS;
  int    fps     = 30;

  /* Determine important sim info from symbols in the sym table. */
  swio_sym_data *tau_data =     swio_sym_get("time_step");
  swio_sym_data *entropy_data = swio_sym_get("entropy");
  swio_sym_data *sim_len_data = swio_sym_get("sim_len");
  swio_sym_data *threads_data = swio_sym_get("threads");
  swio_sym_data *fps_data     = swio_sym_get("fps");

  if (tau_data){
    if (tau_data->type == SYM_NUMERIC){
      tau = *((double *)tau_data->data);
    } else {
      printf("[simulator] Warning, tau data not numeric. Ignoring.\n");
    }
  }
  if (entropy_data){
    if (entropy_data->type == SYM_NUMERIC){
      entropy = *((double *)entropy_data->data);
    } else {
      printf("[simulator] Warning, entropy data not numeric. Ignoring.\n");
    }
  }
  if (sim_len_data){
    if (sim_len_data->type == SYM_NUMERIC){
      sim_len = *((double *)sim_len_data->data);
    } else {
      printf("[simulator] Warning, sim_len data not numeric. Ignoring.\n");
    }
  }
  if (threads_data){
    if (threads_data->type == SYM_NUMERIC){
      threads = (int) (*((double *)threads_data->data));
    } else {
      printf("[simulator] Warning, thread data not numeric. Ignoring.\n");
    }
  }
  if (fps_data){
    if (fps_data->type == SYM_NUMERIC){
      fps = (int) (*((double *)fps_data->data));
      if (fps < 1) fps = 1;
    } else {
      printf("[simulator] Warning, FPS data not numeric. Ignoring.\n");
    }
  }

  sw->tau = tau;
  sw->entropy = entropy;
  sw->sim_len = sim_len;
  sw->threads = threads;
  sw->fps = fps;

}

void _sw_simulate_frame(){

  /* Here we simulate a single frame across a time=sw_internal->tau.     */
  /* Calculations must be made for each insect: how it will react to the */
  /* presence of all other insects. The the entropy specified by the     */
  /* simulation is added as random noise.                                */

  /* This will be multithreaded once it works. */

  // Iterate across all insects and calculate the net force acting on it.
  int i = 0;
  for ( ; i < internal_sw->swarm_size; i++){

    

  }


}
